package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

public class Famine{
    Joystick DRIVER,SHOOTER;
    CANJaguar fl,fr,bl,br;

    public Famine(Joystick DRIVER,Joystick SHOOTER){
        System.out.println("Famine: Initialized with 2 Joystick");
        this.DRIVER=DRIVER;
        this.SHOOTER=SHOOTER;
    }
    public Famine(Joystick DRIVER){
        System.out.println("Famine: Initialized with 1 Joystick");
        this.DRIVER=DRIVER;
    }
    public void tankDrive(double power,int direction,CANJaguar fl,CANJaguar fr,CANJaguar bl,CANJaguar br)throws CANTimeoutException{
        fl.setX(-DRIVER.getRawAxis(Death.DRIVER_LEFT_Y)*power*direction);
        fr.setX(DRIVER.getRawAxis(Death.DRIVER_RIGHT_Y)*power*direction);
        bl.setX(fl.getX());
        br.setX(fr.getX());
    }
    public void tankDrive(double power,CANJaguar fl,CANJaguar fr)throws CANTimeoutException{
        fl.setX(-DRIVER.getRawAxis(Death.DRIVER_LEFT_Y)*power);
        fr.setX(DRIVER.getRawAxis(Death.DRIVER_RIGHT_Y)*power);
    }
    public void tankPickup(CANJaguar pickup)throws CANTimeoutException{
        pickup.setX(!DRIVER.getRawButton(Death.DRIVER_LEFT_BUMPER)?!DRIVER.getRawButton(Death.DRIVER_LEFT_TRIGGER)?0:-1:1);
    }
    public void tankBridgeDrop(Servo s){
        s.setAngle(!DRIVER.getRawButton(Death.DRIVER_BUTTON_B)?!DRIVER.getRawButton(Death.DRIVER_BUTTON_A)?0:120/7.:0);
    }
    public void autoBridge(Servo s, boolean open){
       s.setAngle(open?120/7.:0);
    }
    public void tankShooter(double power,Servo x,CANJaguar y,CANJaguar shoot1,CANJaguar shoot2,CANJaguar pickup)throws CANTimeoutException{
        shoot1.setX(SHOOTER.getRawButton(Death.SHOOTER_LEFT_TRIGGER)?power*Death.SHOOTER_MAX_VOLTAGE:0);
        shoot2.setX(-shoot1.getX());
        pickup.setX(!SHOOTER.getRawButton(Death.SHOOTER_RIGHT_BUMPER)?!SHOOTER.getRawButton(Death.SHOOTER_RIGHT_TRIGGER)?0:-1:1);
    }
    public void autoAim(Servo x,CANJaguar y,CANJaguar shoot,BloodyBacon com,AnalogChannel potX, AnalogChannel potY) throws CANTimeoutException{
        String sho = com.requestInfo("SHOOT-T0");
        double pi = Double.parseDouble(sho.substring(0,sho.indexOf(",")));
        double ya = Double.parseDouble(sho.substring(sho.indexOf(",")+1,sho.indexOf("~")));
        double speed = Double.parseDouble(sho.substring(sho.indexOf("~")));
        x.setAngle(ya);
        while (Math.abs(potY.getAverageVoltage()-pi)<0.1) y.setX(0.25);
        if (DriverStation.getInstance().isOperatorControl()) {
            // Teleop Mode - Allow Manual Override - Wait For Shoot Command
            while (DriverStation.getInstance().isOperatorControl() && DriverStation.getInstance().isEnabled()) {
                x.setAngle(x.getAngle()+SHOOTER.getRawAxis(Death.SHOOTER_LEFT_X)*3+SHOOTER.getRawAxis(Death.SHOOTER_RIGHT_X));
                y.setX(SHOOTER.getRawAxis(Death.SHOOTER_LEFT_Y)*0.5+SHOOTER.getRawAxis(Death.SHOOTER_RIGHT_Y)*0.2);
                if (SHOOTER.getRawButton(Death.SHOOTER_BUTTON_A)) break;
            }
        }
        shoot.changeControlMode(CANJaguar.ControlMode.kVoltage);
        shoot.setX(speed);
        Timer.delay(2);
        shoot.setX(0);
    }
}